{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "l8Rm5122dy2R"
   },
   "source": [
    "## **Analytic Antipodal Grasps**"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "VsbCH_XUJDCN"
   },
   "source": [
    "## Notebook Setup \n",
    "The following cell will install Drake, checkout the manipulation repository, and set up the path (only if necessary).\n",
    "- On Google's Colaboratory, this **will take approximately two minutes** on the first time it runs (to provision the machine), but should only need to reinstall once every 12 hours.  \n",
    "\n",
    "More details are available [here](http://manipulation.mit.edu/drake.html)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "id": "opM0QN-CPwry"
   },
   "outputs": [],
   "source": [
    "import importlib\n",
    "import os, sys\n",
    "from urllib.request import urlretrieve\n",
    "\n",
    "if 'google.colab' in sys.modules and importlib.util.find_spec('manipulation') is None:\n",
    "    urlretrieve(f\"http://manipulation.csail.mit.edu/scripts/setup/setup_manipulation_colab.py\",\n",
    "                \"setup_manipulation_colab.py\")\n",
    "    from setup_manipulation_colab import setup_manipulation\n",
    "    setup_manipulation(manipulation_sha='db19656799c11a58bc37dd20604ad38eafb09c62', drake_version='20201007', drake_build='nightly')\n",
    "\n",
    "from IPython import get_ipython\n",
    "running_as_notebook = get_ipython() and hasattr(get_ipython(), 'kernel')\n",
    "\n",
    "import numpy as np\n",
    "\n",
    "from pydrake.all import(\n",
    "    Variable, sin, cos, Evaluate, Jacobian, atan, MathematicalProgram, Solve, eq\n",
    ")\n",
    "\n",
    "import matplotlib.pyplot as plt, mpld3\n",
    "if running_as_notebook:\n",
    "  mpld3.enable_notebook()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "_4KTk534ex9e"
   },
   "source": [
    "## Introduction to Symbolic Differentiation \n",
    "\n",
    "For this assignment, you will need [symbolic differentiation](https://en.wikipedia.org/wiki/Computer_algebra), supported by Drake's symbolic library. We will demonstrate how to use it with a simple function: \n",
    "$$T=\\cos^2(x) + y^5$$\n",
    "\n",
    "and it's Jacobian (first-order derivative), \n",
    "$$J = \\begin{pmatrix} \\frac{\\partial T}{\\partial x} & \\frac{\\partial T}{\\partial y} \\end{pmatrix}=\\begin{pmatrix} -2\\cos(x)\\sin(x) & 5y^4 \\end{pmatrix}$$\n",
    "\n",
    "as well as the Hessian (second-order derivative), \n",
    "$$H = \\begin{pmatrix} \\frac{\\partial^2 T}{\\partial x^2} & \\frac{\\partial^2 T}{\\partial x \\partial y} \\\\ \\frac{\\partial^2 T}{\\partial y \\partial x} & \\frac{\\partial^2 T}{\\partial y^2} \\end{pmatrix}=\\begin{pmatrix} 2 \\sin^2(x) - 2\\cos^2(x) & 0 \\\\ 0 & 20y^3 \\end{pmatrix}$$\n",
    "\n",
    "Below are some snippets of how to define symbolic variables, differentiate expressions, and evaluate them using numerical values. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "id": "FbI9kuNKe9fb"
   },
   "outputs": [],
   "source": [
    "# 1. Symbolic variables are defined\n",
    "x = Variable('x')\n",
    "y = Variable('y')\n",
    "\n",
    "# 2. Expressions can be written by composing operations on Variables. \n",
    "T = cos(x) ** 2.0 + y ** 5.0\n",
    "print(T)\n",
    "\n",
    "# 3. Use Evaluate to query the numerical value of the expression given the variable values. \n",
    "# Use function for multi-dimensional quantities\n",
    "print(Evaluate(np.array([T]), {x: 3.0, y:5.0}))\n",
    "# Use method for scalar quantities \n",
    "print(T.Evaluate({x: 3.0, y:5.0}))\n",
    "\n",
    "# 4. Differentiate a quantity using Jacobian, or Differentiate. \n",
    "J = np.array([T.Differentiate(x), T.Differentiate(y)])\n",
    "print(J)\n",
    "# Use method for scalar quantities \n",
    "J = T.Jacobian([x, y])\n",
    "print(J)\n",
    "print(Evaluate(J, {x: 3.0, y:5.0}))\n",
    "\n",
    "# Use function for taking Jacobian of multi-dimensional quantities.\n",
    "H = Jacobian(J, [x, y])\n",
    "print(H)\n",
    "print(Evaluate(H, {x: 3.0, y: 5.0}))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "A7Xm7hVkhbF0"
   },
   "source": [
    "Are the symbolic values of the Jacobian and Hessian what you expect? "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "KncJ9HEed94E"
   },
   "source": [
    "## The Cycloidal Gear\n",
    "\n",
    "Now we enter the main part of the problem. \n",
    "\n",
    "After graduating from MIT, you decide to work at a company producing cutting-edge [hypercycloidal gears](https://youtu.be/MBWkibie_5I?t=74). You are in charge of designing a robotic pick-and-place system for these parts. In order to reliably grasp the gears, you decide to use your knowledge of antipodal points. \n",
    "\n",
    "The mechanical design department gave you a pretty ugly parametric equation for what the shape looks like, which we won't even bother writing in latex! Instead, we provided it via the function `shape`. \n",
    "\n",
    "Given a angle in polar coordinates (parameter $t$), it returns $p(t)=[x(t),y(t)]$, a position in 2D.  \n",
    "\n",
    "The below cell implements the function and shows you what the gear part looks like. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "id": "K9tnarjr8FnB"
   },
   "outputs": [],
   "source": [
    "def shape(t):\n",
    "  x = (10*cos(t))-(1.5*cos(t+atan(sin(-9*t)/((4/3)-cos(-9*t)))))-(0.75*cos(10*t))\n",
    "  y = (-10*sin(t))+(1.5*sin(t+atan(sin(-9*t)/((4/3)-cos(-9*t)))))+(0.75*sin(10*t))\n",
    "  return np.array([x, y])\n",
    "\n",
    "def plot_gear():\n",
    "  theta = np.linspace(0, 2*np.pi, 500)\n",
    "  gear_shape = []\n",
    "  for i in range(500):\n",
    "    gear_shape.append(Evaluate(shape(theta[i])).squeeze())\n",
    "  gear_shape = np.array(gear_shape)\n",
    "  plt.axis(\"equal\")\n",
    "  plt.plot(gear_shape[:,0], gear_shape[:,1], 'k-')\n",
    "\n",
    "plot_gear()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "m-W34GnXCG_N"
   },
   "source": [
    "## Grasp Energy Function\n",
    "\n",
    "How can we analytically find a pair of antipodal points given the parametric equation of a shape? We make the following claim: \n",
    "\n",
    "**Claim**: Let $p(t_1)$ and $p(t_2)$ be a pair of antipodal points given in parametric space. Then $t_1$ and $t_2$ are critical points of the following energy function:\n",
    "$$E=\\frac{1}{2}\\kappa\\|p(t_1)-p(t_2)\\|^2$$\n",
    "\n",
    "that is, they satisfy $\\frac{\\partial E}{\\partial \\mathbf{t}}=[0, 0]$ where $\\mathbf{t}=[t_1,t_2]$. \n",
    "\n",
    "For the subsequent problems, you may assume $\\kappa=2$. \n",
    "\n",
    "**Problem 5.1.a** [2pts]: Prove the claim. \\\\\n",
    "**Problem 5.1.b** [2pts]: Prove that the converse may not necessarily hold. \n",
    "\n",
    "HINT: The derivative of $p(t)$ respect to $t$ gives the tangent 'velocity' vector: $v(t)=p'(t)$\n",
    "\n",
    "Write down your answer in a paper / pdf file, and submit to the Gradescope written submission section! "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "iNo0LiKyq0CO"
   },
   "source": [
    "## Implementation\n",
    "\n",
    "**Problem 5.1.c** [4pts]\n",
    "Using this knowledge, we will write a Mathematical Program to find the antipodal points. Since we are looking for $t_1$ and $t_2$ such that the Jacobians is a zero vector, we are solving a root finding problem. Problems of this nature can still be transcribed as an instance of a Mathematical program;  it simply doesn't have a cost. \n",
    "\n",
    "We will write down our problem as follows: \n",
    "\n",
    "\\begin{aligned} \\text{find} \\quad & \\mathbf{t}  \\\\\n",
    "                  \\text{s.t.} \\quad &  \\frac{\\partial E}{\\partial \\mathbf{t}}(\\mathbf{t}) = \\mathbf{0} \\\\ \n",
    "                  \\quad & 0 \\leq \\mathbf{t} \\leq 2\\pi  \\\\\n",
    "                  \\quad & t_1 - t_2 \\geq \\varepsilon\n",
    "                  \\end{aligned}\n",
    "\n",
    "The first constraint makes sure that they are critical points of the energy function, while the last two makes sure the points are not overlapping. You will write the following outer loop to check for the validity of solutions.\n",
    "\n",
    "1. Pick a random guess for $\\mathbf{t}$ using [SetInitialGuess](https://drake.mit.edu/pydrake/pydrake.solvers.mathematicalprogram.html?highlight=setinitialguess#pydrake.solvers.mathematicalprogram.MathematicalProgram.SetInitialGuess) by uniform sampling over $[0, 2\\pi]$ (use `np.random.rand(2)`). \n",
    "2. Using `MathematicalProgram`, solve the above problem. Remember there is no cost in this problem, so we simply only add the constraints. \n",
    "3. If the solution is not valid (i.e. problem doesn't return success), repeat 1-2 with random guesses until a valid solution is found. \n",
    "4. If a valid solution $\\mathbf{t}^*$ is found, return the Eigenvalues of the Hessian of $E$ at $\\mathbf{t}^*$. (Use `np.linalg.eigvals`)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "id": "WYPS22sY80WD"
   },
   "outputs": [],
   "source": [
    "def find_antipodal_pts(shape):\n",
    "  \"\"\"\n",
    "  Finds antipodal points given the parametric function that describes the shape of the object.\n",
    "  Args:\n",
    "    - shape: function from parametric space t to position R2. \n",
    "  Returns:\n",
    "    - result: 2-dim np array that contains antipodal grasp locations parametrized by [t1, t2] \n",
    "    - H_eig: 2-dim np array that contains eigenvalues of the Hessian. \n",
    "  \"\"\"\n",
    "\n",
    "  eps = 1e-3 # do not modify, but use it for epsilon variable above. \n",
    "\n",
    "  ## Fill your code here \n",
    "  result = np.array([0., 0.]) # modify here\n",
    "  H_eig = np.array([0., 0.])  # modify here \n",
    "\n",
    "  return result, H_eig"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "LuOUVR5Vys3p"
   },
   "source": [
    "You can run the cell below to check the correctnes of your implementation. As the constraint is nonlinear, it might take some time to compute. (Typically, the solve time will still be less than 2~3 seconds)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "id": "Hzm6QrdGT88d"
   },
   "outputs": [],
   "source": [
    "def plot_antipodal_pts(pts, shape):\n",
    "  antipodal_pts = []\n",
    "  for i in range(2):\n",
    "    val = Evaluate(shape(pts[i])).squeeze()\n",
    "    antipodal_pts.append(val)\n",
    "  antipodal_pts = np.array(antipodal_pts)\n",
    "  plt.scatter(antipodal_pts[:,0], antipodal_pts[:,1], color='red')\n",
    "\n",
    "plot_gear()\n",
    "result, H_eig = find_antipodal_pts(shape)\n",
    "plot_antipodal_pts(result, shape)\n",
    "print(H_eig)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "tvstL9DDHIk3"
   },
   "source": [
    "## Hessian Analysis\n",
    "\n",
    "Why did we implement the Hessian? You may remember that if the Hessian is used for the second-derivative test. For a function $f(x)$ with a critical point $x^*$, this critical point is:\n",
    "- A local minima if the Hessian is positive-definite (i.e. all positive eigenvalues)\n",
    "- A local maxima if the Hessian is negative-definite (i.e. all negative eigenvalues)\n",
    "- A saddle point if the Hessian has mixed positive / negative eigenvalues. \n",
    "\n",
    "**Problem 5.1.d** [2pts] Describe what grasps the local minima, maxima, and saddle points correspond to in terms of the geometry of the object. In a very simple sentence, explain why you might prefer one configuration over another. \n",
    "\n",
    "HINT: The cell below will visualize each of the cases. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "id": "MF16oJya8-WM"
   },
   "outputs": [],
   "source": [
    "if (running_as_notebook):\n",
    "    plt.subplot(1,3,1)\n",
    "    plot_gear()\n",
    "    plt.title(\"Local Minima\")\n",
    "    np.random.seed(45)\n",
    "    while True:\n",
    "      result, H_eig = find_antipodal_pts(shape)\n",
    "      if ((H_eig > 0).all()):\n",
    "        break\n",
    "    plot_antipodal_pts(result, shape)\n",
    "\n",
    "    plt.subplot(1,3,2)\n",
    "    plot_gear()\n",
    "    plt.title(\"Local Maxima\")\n",
    "    np.random.seed(4)\n",
    "    while True:\n",
    "      result, H_eig = find_antipodal_pts(shape)\n",
    "      if ((H_eig < 0).all()):\n",
    "        break\n",
    "    plot_antipodal_pts(result, shape)\n",
    "\n",
    "    plt.subplot(1,3,3)\n",
    "    plot_gear()\n",
    "    plt.title(\"Saddle Point\")\n",
    "    np.random.seed(13)\n",
    "    while True:\n",
    "      result, H_eig = find_antipodal_pts(shape)\n",
    "      if ((H_eig[0] > 0) and (H_eig[1] < 0)):\n",
    "        break\n",
    "    plot_antipodal_pts(result, shape)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "MwE8yNg58VQN"
   },
   "source": [
    "## How will this notebook be Graded?\n",
    "\n",
    "If you are enrolled in the class, this notebook will be graded using [Gradescope](www.gradescope.com). You should have gotten the enrollement code on our announcement in Piazza. \n",
    "\n",
    "For submission of this assignment, you must do two things. \n",
    "- Download and submit the notebook `analytic_antipodal_grasps.ipynb` to Gradescope's notebook submission section, along with your notebook for the other problems.\n",
    "- Write down your answers to 5.1.a, 5.1.b, and 5.1.d to a separately pdf file and submit it to Gradescope's written submission section. \n",
    "\n",
    "We will evaluate the local functions in the notebook to see if the function behaves as we have expected. For this exercise, the rubric is as follows:\n",
    "- [2 pts] 5.1.a is answered correctly.\n",
    "- [2 pts] 5.1.b is answered correctly. \n",
    "- [4 pts] `find_antipodal_points` must be implemented correctly.\n",
    "- [2 pts] 5.1.d is answered correctly."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "id": "xj5nAh4g8VQO"
   },
   "outputs": [],
   "source": [
    "from manipulation.exercises.clutter.test_analytic_grasp import TestAnalyticGrasp\n",
    "from manipulation.exercises.grader import Grader \n",
    "\n",
    "Grader.grade_output([TestAnalyticGrasp], [locals()], 'results.json')\n",
    "Grader.print_test_results('results.json')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "celltoolbar": "Tags",
  "colab": {
   "collapsed_sections": [],
   "name": "analytic_antipodal_grasps.ipynb",
   "provenance": []
  },
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}